// 显示摄像头中的白线

#include <vector>
#include <opencv2/opencv.hpp>
#include "cmvvision.h"
#include "calibration.h"
#include "gridmap.h"

static unsigned getWhiteIndex(const std::vector<cmv::Color> &colors)
{
	for (unsigned i = 0; i < colors.size() ; ++i) {
		if (colors[i].name == "white") {
			return i;
		}
	}
	return -1;
}

int main(int argc, char **argv)
{
	std::vector<cmv::Color> colors;
	cmv::Vision *vision;
	cmv::ImageYUV422 image;
	cv::VideoCapture capture(0);
	Calibration calibration("calibration.txt");
	GridMap gridmap("map.txt");
	cv::Mat mat;
	cv::Mat mapMat;
	bool initialized = false;
	unsigned white;
	const cmv::Pixel *pixels;
	int numPixels;
	int xToRobot, yToRobot;
	int xPixel, yPixel;

	if (!calibration.isValid()) {
		std::cerr << "calibration init error!" << std::endl;
		return 1;
	}

	// 检查摄像头是否打开
	if (!capture.isOpened()) {
		std::cerr << "can't open camera!" << std::endl;
		return 1;
	}

	// 初始化cmvision
	colors = cmv::loadConfig("colors.txt");
	white = getWhiteIndex(colors);
	if (white == -1) {
		std::cerr << "white color not found in colors.txt" << std::endl;
	}
	vision = new cmv::Vision;
	if (!vision) {
		std::cerr << "cmvision can't be created!" << std::endl;
	}

	// 建立显示窗口
	cv::namedWindow("camera");
	cv::namedWindow("map");

	while (true) {
		capture >> mat;
		if (!cmv::loadImageYUV422FromMat(mat, image)) {
			continue;
		}
		if (!initialized) {
			vision->initialize(colors, image.width, image.height);
			mapMat = cv::Mat(gridmap.getHeight(), gridmap.getWidth(), CV_8UC3);
			initialized = true;
		}
		vision->classifyFrame(image.data, calibration.getRobotPixelX(), calibration.getRobotPixelY(),
			0, 50, white, pixels, numPixels);
		mapMat = gridmap.renderMap();
		for (int i = 0; i < numPixels; ++i) {
			calibration.pixToPose(pixels[i].x, pixels[i].y, 0.0, xToRobot, yToRobot);
			xPixel = 234 - yToRobot;
			yPixel = 335 - xToRobot;
			if (xPixel >= 0 && xPixel < gridmap.getWidth()
					&& yPixel >= 0 && yPixel < gridmap.getHeight()) {
						mapMat.at<cv::Vec3b>(yPixel, xPixel) = cv::Vec3b(0, 0, 255);
			}
		}

		cv::imshow("camera", mat);
		cv::imshow("map", mapMat);
		if (cv::waitKey(30) != -1) {
			break;
		}
	}

	delete vision;

	return 0;
}
